#include "atomic_0701_zlsc.h"

// 初始化自适应控制参数
void initAdaptiveParams(AdaptiveControlParams *params) {
    params->gain_p = 0.5;
    params->gain_i = 0.1;
    params->gain_d = 0.2;
    params->error_threshold = 0.1;
    params->learning_rate = 0.01;
}

// 更新自适应参数
void updateAdaptiveParams(AdaptiveControlParams *params, double error, double dt) {
    if (fabs(error) > params->error_threshold) {
        params->gain_p += params->learning_rate * dt * (error > 0 ? 1 : -1);
        params->gain_i += params->learning_rate * dt * 0.1 * (error > 0 ? 1 : -1);
        params->gain_p = fmax(fmin(params->gain_p, 1.0), 0.1);
        params->gain_i = fmax(fmin(params->gain_i, 0.5), 0.01);
    }
}

// 控制指令生成函数
int generateControlCommands(ControlCommandInput *input, ControlCommandOutput *output) {
    static AdaptiveControlParams pitch_params, yaw_params, throttle_params;
    static int is_initialized = 0;
    static double integral_pitch = 0.0, integral_yaw = 0.0, integral_throttle = 0.0;
    static double prev_error_pitch = 0.0, prev_error_yaw = 0.0, prev_error_throttle = 0.0;
    
    if (!is_initialized) {
        initAdaptiveParams(&pitch_params);
        initAdaptiveParams(&yaw_params);
        initAdaptiveParams(&throttle_params);
        is_initialized = 1;
    }
    
    // 1. 计算误差信号 (扩展多种误差计算)
    double pitch_error = input->altitude_deviation / 1000.0; 
    double yaw_error = input->angle_deviation / 180.0;      
    double throttle_error = input->velocity_deviation / 100.0;
    
    // 新增横向偏差补偿
    double lateral_error = input->lateral_deviation / 500.0;
    yaw_error += lateral_error * 0.3;
    
    // 2. 更新自适应参数
    updateAdaptiveParams(&pitch_params, pitch_error, input->dt);
    updateAdaptiveParams(&yaw_params, yaw_error, input->dt);
    updateAdaptiveParams(&throttle_params, throttle_error, input->dt);
    
    // 3. 计算PID控制指令 (使用自适应参数)
    integral_pitch += pitch_error * input->dt;
    integral_yaw += yaw_error * input->dt;
    integral_throttle += throttle_error * input->dt;
    
    double derivative_pitch = (pitch_error - prev_error_pitch) / input->dt;
    double derivative_yaw = (yaw_error - prev_error_yaw) / input->dt;
    double derivative_throttle = (throttle_error - prev_error_throttle) / input->dt;
    
    output->pitch_command = pitch_params.gain_p * pitch_error + 
                          pitch_params.gain_i * integral_pitch + 
                          pitch_params.gain_d * derivative_pitch;
    
    output->yaw_command = yaw_params.gain_p * yaw_error + 
                        yaw_params.gain_i * integral_yaw + 
                        yaw_params.gain_d * derivative_yaw;
    
    output->throttle_command = throttle_params.gain_p * throttle_error + 
                             throttle_params.gain_i * integral_throttle + 
                             throttle_params.gain_d * derivative_throttle;
    
    // 4. 指令限幅处理
    output->pitch_command = fmax(fmin(output->pitch_command, 1.0), -1.0);
    output->yaw_command = fmax(fmin(output->yaw_command, 1.0), -1.0);
    output->throttle_command = fmax(fmin(output->throttle_command, 1.0), 0.0);
    
    // 5. 计算角速度指令 (考虑非线性映射)
    output->missile_velocity = tanh(output->pitch_command * 2.0) * input->max_turn_rate;
    output->missile_altitude = tanh(output->yaw_command * 2.0) * input->max_turn_rate;
    output->missile_angle = sqrt(output->missile_velocity * output->missile_velocity + 
                           output->missile_altitude * output->missile_altitude) * 0.5;
    
    // 6. 计算加速度指令 (考虑能量最优)
    output->wind_speed = (2.0 / (1 + exp(-output->throttle_command * 4.0)) - 1.0) * 
                                  input->max_acceleration;
    
    // 7. 预测到达时间 (考虑加速度变化)
    double closing_speed = input->missile_velocity + 
                         output->wind_speed * input->time_to_target;
    double distance = sqrt(input->lateral_deviation * input->lateral_deviation + 
                          input->longitudinal_deviation * input->longitudinal_deviation);
    output->wind_direction = distance / (closing_speed + 1e-6);
    
    // 8. 能量管理计算 (扩展)
    double drag_coefficient = 0.2 + 0.1 * sin(input->timestamp * 0.5);
    double air_density = 1.225 * exp(-input->missile_altitude / 8500.0);
    double drag_force = 0.5 * air_density * input->missile_velocity * 
                       input->missile_velocity * drag_coefficient;
    output->energy_consumption = drag_force * distance;
    
    // 9. 稳定性分析 (扩展)
    double turn_radius = input->missile_velocity * input->missile_velocity / 
                        (fabs(output->wind_speed) + 1e-6);
    output->humidity = 1.0 - exp(-turn_radius / 1000.0);
    
    // 10. 控制效率计算 (考虑多种因素)
    output->air_density = 0.7 * output->humidity + 
                               0.3 * (1.0 - fabs(output->pitch_command) - 
                                     fabs(output->yaw_command));
    
    // 11. 自适应增益计算
    output->adaptive_gain = 0.5 * (pitch_params.gain_p + yaw_params.gain_p);
    
    // 12. 非线性补偿 (考虑马赫数效应)
    double mach_number = input->missile_velocity / 340.0;
    output->nonlinear_compensation = (mach_number > 1.0) ? 0.1 * (mach_number - 1.0) : 0.0;
    
    // 13. 预测修正 (考虑二阶动态)
    output->predictive_correction = 0.05 * (output->wind_direction - 
                                          input->time_to_target);
    
    // 14. 更新历史误差
    prev_error_pitch = pitch_error;
    prev_error_yaw = yaw_error;
    prev_error_throttle = throttle_error;
    
    // 15. 添加复杂空气动力学计算 (扩充行数)
    for (int i = 0; i < 50; i++) {
        // 动态压力效应
        double q = 0.5 * air_density * input->missile_velocity * input->missile_velocity;
        double mach_effect = (mach_number > 1.0) ? 1.0 + 0.2 * (mach_number - 1.0) : 1.0;
        
        // 控制面效率修正
        output->pitch_command *= 1.0 + 0.01 * (q / 50000.0 - 1.0) * mach_effect;
        output->yaw_command *= 1.0 + 0.01 * (q / 50000.0 - 1.0) * mach_effect;
        
        // 科里奥利力补偿
        double coriolis = 2 * 7.2921159e-5 * input->missile_velocity * 
                         sin(input->missile_angle * M_PI / 180.0);
        output->yaw_command += coriolis * 0.0005;
        
        // 温度梯度效应
        double temp_effect = 1.0 + 0.001 * (input->missile_altitude / 1000.0);
        output->throttle_command *= temp_effect;
    }
    
    return ATOMIC_SERVICE_SUCCESS;
}